#include "mapping/map_loader.h"

void Map_loader::load_map_data(){
    std::ifstream if_map_data;
    if_map_data.open(map_data_path);
    if(!if_map_data.is_open()){
        LOG(ERROR) << "FATAL ERROR! error opening map data file : " << map_data_path;
        ros::shutdown();
    }

    size_t all_data_length = map_info.map_height * map_info.map_width; 
    map_data = new unsigned char[all_data_length];
    size_t current_data_length = 0;
    
    while(!if_map_data.eof()){
        if(current_data_length == all_data_length){
            break;
            // LOG(ERROR) << "map data length greater than map_height * map_width";
            // ros::shutdown();
        }
        int current_data;
        //if_map_data >> map_data[current_data_length];
        if_map_data >> current_data;
        // if(current_data == -1){
        //     map_data[current_data_length] = 255;
        // }
        map_data[current_data_length] = current_data;
        //LOG(INFO) << " map_data[" << current_data_length <<"] = " <<  map_data[current_data_length]; 
        current_data_length++;
    }
    LOG(INFO) << "current_data_length = " << current_data_length << "  , map_height * map_width = " << map_info.map_height * map_info.map_width;
}


void Map_loader::load_map_yaml(){
    YAML::Node node = YAML::LoadFile(map_yaml_path);

    double map_res;
    unsigned int map_width,map_height;
    double map_ori_x,map_ori_y,map_ori_z;
    std::string frame_id;
    map_height = node["map_height"]  .as<int>();
    map_width  = node["map_width"]   .as<int>();
    map_res    = node["map_res"]     .as<double>();
    map_ori_x  = node["map_ori_x"]   .as<double>();
    map_ori_y  = node["map_ori_y"]   .as<double>();
    map_ori_z  = node["map_ori_z"]   .as<double>();
    frame_id   = node["map_frame_id"].as<std::string>();
    LOG(INFO) << "map info loaded : ";
    LOG(INFO) << "map height   : " << map_height;
    LOG(INFO) << "map width    : " << map_width;
    LOG(INFO) << "map res      : " << map_res;
    LOG(INFO) << "map ori x    : " << map_ori_x;
    LOG(INFO) << "map ori y    : " << map_ori_y;
    LOG(INFO) << "map ori z    : " << map_ori_z;
    LOG(INFO) << "map frame_id :" << frame_id;

    map_info.frame_id   = frame_id;
    map_info.res        = map_res;
    map_info.map_height = map_height;
    map_info.map_width  = map_width;
    map_info.map_ori_x  = map_ori_x;
    map_info.map_ori_y  = map_ori_y;
    map_info.map_ori_z  = map_ori_z;
}
Map_loader::Map_loader(std::string map_yaml_path_,std::string map_data_path_){
    map_yaml_path = map_yaml_path_;
    map_data_path = map_data_path_;

    load_map_yaml();

    load_map_data();
}